#include "Algorithm/subdivision_of_polygon.h"



static int is_point_in_triangle_subdivision_of_polygon(double*p,double *p1,double*p2,double*p3,double*n)
{
    int re=1;
    double* ps[3]={p1,p2,p3};
    for(int i=0;i<3;i++)
    {
        int j=(i+2)%3;
        double temp_p1[3]={ps[i][0]-ps[j][0],ps[i][1]-ps[j][1],ps[i][2]-ps[j][2]};
        double temp_p2[3]={p[0]-ps[i][0],p[1]-ps[i][1],p[2]-ps[i][2]};
        double* temp_p3=out_product(temp_p1,temp_p2);
        if(inner_product(temp_p3,n,3)<0)
        {
            re=0;
            SAFE_FREE(temp_p3);   
            break;
        }
        SAFE_FREE(temp_p3);   
    } 
    return re;
}

double * compute_plane_normal_of_jordan_curve(double** v,int len )
{
    double* re=(double*)malloc(sizeof(double)*3); 
    memset(re,0,sizeof(double)*3);
    for(int i=0;i<len;i++)
    {
        int j=(i-1+len)%len,k=(i+1)%len;
        double a[3]={v[i][0]-v[j][0],v[i][1]-v[j][1],v[i][2]-v[j][2]};
        double b[3]={v[k][0]-v[i][0],v[k][1]-v[i][1],v[k][2]-v[i][2]};
        if(normalize(a,3)&&normalize(b,3))
        {
            double * n=out_product(a,b);
            double temp=acos(inner_product(a,b,3));
            if(normalize(n,3)==1)
            {
                re[0]+=temp*n[0];re[1]+=temp*n[1];re[2]+=temp*n[2];
            }
        //printf("n : %lf %lf %lf %lf\n",n[0],n[1],n[2],temp);
            SAFE_FREE(n);
        }
        
    } 
    return re;
}


//
//是否是内角
//
int is_acute_angle_in_polygon(double**v,int len,double* n,int i)
{
    int j=(i-1+len)%len,k=(i+1)%len;
    double a[3]={v[i][0]-v[j][0],v[i][1]-v[j][1],v[i][2]-v[j][2]};
    double b[3]={v[k][0]-v[i][0],v[k][1]-v[i][1],v[k][2]-v[i][2]};
    //normalize(a);normalize(b);
    double* n1=out_product(a,b);
    double re=0;
    if(inner_product(n1,n,3)>=0)
    {
        re=1;
    }
    SAFE_FREE(n1); 
    return re;
}
//是否能够建立三角形

int is_subdivision_of_polygon_legal_angle1(double **v,int len,double*n,int i)
{
    int j=(i-1+len)%len,k=(i+1)%len;

    double a[3]={v[i][0]-v[j][0],v[i][1]-v[j][1],v[i][2]-v[j][2]};
    double b[3]={v[k][0]-v[i][0],v[k][1]-v[i][1],v[k][2]-v[i][2]};
    //normalize(a);normalize(b);
    double* n1=out_product(a,b);
    //printf("%lf %lf %lf\n",n1[0],n1[1],n1[2]);
    double re=0;
    if(inner_product(n1,n,3)>=0)
    {
        re=1;
        for(int l=0;l<len-3;l++)
        {
            int m=(l+k+1)%len;
            if(is_point_in_triangle_subdivision_of_polygon(v[m],v[j],v[i],v[k],n)==1)
            {
                re=0;
                break;
            }
        } 
    }
    else
    {
        re=0; 
    }
    SAFE_FREE(n1); 
    return re;
}




typedef struct Sub_Poly_Ver{
    double* p;
    int i;
    double t;
}Sub_Poly_Ver;
static inline sub_poly_ver_init(Sub_Poly_Ver* spv)
{
    spv->p=NULL;
    spv->i=0;
    spv->t=0;
}

static void subdivision_of_polygon1( Sub_Poly_Ver* spvs,int len,int **s,double* n)
{
    if(len<3)
    {
        return ;
    }
    if(len==3)
    {
        s[0][0]=spvs[0].i;
        s[0][1]=spvs[1].i;
        s[0][2]=spvs[2].i;
        return ;
    }
    double** v=(double**)malloc(sizeof(double*)*len);
    for(int i=0;i<len;i++)
    {
        v[i]=spvs[i].p;
       // printf("spvs i:%d\n",spvs[i].i);
       // printf("%lf %lf %lf\n",v[i][0],v[i][1],v[i][2]);
    } 
    int an=0;
    for(int i=0;i<len;i++)
    {
        if(is_subdivision_of_polygon_legal_angle1(v,len,n,i))
        {
            an=i;
          //  printf("find an :%d\n",an);
            break;
        } 
    }

    free(v);
   // printf("here1\n");
    int ia=(an-1+len)%len,ib=(an+1+len)%len; 
    s[0][0]=spvs[ia].i;
    s[0][1]=spvs[an].i;
    s[0][2]=spvs[ib].i;
    
    int temp_i=0;
    Sub_Poly_Ver* spvs1=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*(len-1));
    for(int i=0;i<len;i++)
    {
        if(i==an)
        {
            continue;
        }
        spvs1[temp_i]=spvs[i];
        temp_i++; 
    }
    subdivision_of_polygon1(spvs1,len-1,&(s[1]),n);
    free(spvs1);
}


void subdivision_of_polygon(double ** v,int len,int ** s)
{
    double * n=compute_plane_normal_of_jordan_curve(v,len);
    Sub_Poly_Ver* spvs=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*len);
    for(int i=0;i<len;i++)
    {
        spvs[i].p=v[i];
        spvs[i].i=i;
    } 
    subdivision_of_polygon1(spvs,len,s,n); 
    free(spvs);
    SAFE_FREE(n);
}

static inline int is_three_point_in_one_line(double* p1,double* p2,double* p3)
{
    double temp_p1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    double temp_p2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};
    //normalize 
    double* p= out_product(temp_p1,temp_p2);
    int re=0;
    if(fabs(p[0])<1e-8&&fabs(p[1])<1e-8&&fabs(p[2])<1e-8)
    {re=1;}
    SAFE_FREE(p);   
    return re; 
}
void optimization_subdivision_of_polygon(double** v,int len ,int **s)
{
    if(len==3)
    {
        s[0][0]=0;s[0][1]=1;s[0][2]=2;
        return ;
    }
    int *mark=(int*)malloc(sizeof(int)*len);
    for(int i=0;i<len;i++)
    {
         mark[i]=i;
    }
    int len1=0;
    for(int i=0;i<len;i++)
    {
        int j=(i+1)%len,k=(i+2)%len;
        if(is_three_point_in_one_line(v[i],v[j],v[k])==1 )
        {
            //
            mark[j]=-1;
            printf("j:%d is marked\n",j);
            len1++;
        }
    } 
    double** v1=(double**)malloc(sizeof(double*)*(len-len1));
    int *index=(int*)malloc(sizeof(int)*(len-len1));
    int temp_i=0;
    for(int i=0;i<len;i++)
    {
        if(mark[i]!=-1)
        {
            v1[temp_i]=v[i];
            index[temp_i]=i;
            temp_i++;
        }
    }
    subdivision_of_polygon(v1,len-len1,s);
    //printf("fds\n");
    int len2=len-len1-2;
    for(int i=0;i<len2;i++)
    {
        for(int j=0;j<3;j++)
        {
            s[i][j]=index[s[i][j]];
            //printf("%d ",s[i][j]);

        }
        //printf("\n");
    }

    //printf("hree\n");
    // for(int i=0;i<4;i++)
    // {
    //     printf("%d %d %d\n",s[i][0],s[i][1],s[i][2]);
    // }

    //alter s
    for(int i=0;i<len;i++)
    {
        if(mark[i]==-1)
        {
            int k1=i;
            for(int j=1;j<len;j++)
            {
                k1=(i-j+len)%len;
                if(mark[k1]!=-1)
                {
                    break;
                }  
            }
            int k2=i;
            for(int j=1;j<len;j++)
            {
                k2=(i+j)%len;
                if(mark[k2]!=-1)
                {
                    break;
                }
            }
            //printf("k1:%d k2:%d\n",k1,k2);
            int l1=-1,l2=-1;
            for(int j=0;j<len2;j++)
            {
                l1=-1;l2=-1;
                for(int k=0;k<3;k++)
                {
                    s[j][k]==k1?(l1=k):(s[j][k]==k2?(l2=k):(l2=l2));
                }
                if(l1!=-1&&l2!=-1)
                {
                    //printf("zheli alter\n");
                    s[len2][0]=s[j][0];s[len2][1]=s[j][1];s[len2][2]=s[j][2];
                    s[j][l1]=i;
                    s[len2][l2]=i;
                    break;
                }
            }
            mark[i]=i;
            len2++;
        }
    }
    free(index);
    free(v1);
    free(mark);
}
//
//用扫描射线的网格剖分算法
//

void subdivision_of_polygon_with_edges(Sub_Poly_Ver* spvs,int len,int* bounds,int bounds_n,
    int**edges,int edges_n)
{

    
    


}
